CON
        _clkmode = xtal1 + pll16x                                               
        _xinfreq = 5_000_000

        ServoCh1 = 16     ' CHANGE THESE TO MAKE STUFF EASIER, LATER. *TODO*
        ServoCh2 = 17
        ServoCh3 = 18
        ServoCh4 = 19
        STP      = 1500
         
        LFWD     = 2000
        RFWD     = 1000
        LRVRS    = 1000
        RRVRS    = 2000
         
        FORWARD  = 1
        REVERSE  = 2
        LEFT     = 3
        RIGHT    = 4
        STOP     = 5
        
VAR
  long stack[9]
  byte arm_servo
  byte clamp_servo
  
OBJ
  pst : "Parallax Serial Terminal"
  PS2 : "PS2_Controller"
  Num : "Simple_Numbers"
  servo : "Servo32v7"
  func : "Functions"
  xbee : "XBee_Object"
  
PUB Main
  'servo.start
  'func.initialize
  pst.start(115200)
  pst.str(String("Beginning program for testing the PS2 controller...",13))
  arm_servo := 120
  clamp_servo := 90
  {coginit(1,MoveMotor(20,arm_servo),@stack)
  coginit(2,MoveMotor(21,90),@stack+100)
  waitcnt(clkfreq + cnt)}
  ps2.start(12, 5000)
  repeat
    PS2_Interpret 

  
PUB PS2_Interpret
    waitcnt(clkfreq/4 + cnt)
    if (PS2.get_Data1 == $FFF75A73)
      pst.str(String("Pressed: START",13))
      xbee.start(4,3,0,9600)
    if (PS2.get_Data1 == $FFFE5A73)
      pst.str(String("Pressed: SELECT",13))
      xbee.stop
    ' End of bs.
    {if (PS2.get_Data1 == $BFFF5A73) ' X
      pst.str(String("Sent: 6",13))
      xbee.tx(6)
    if (PS2.get_data1 == $DFFF5A73) ' O
      xbee.tx(7)
      pst.str(String("Sent: 7",13))}
    if (PS2.get_RightY <> 0) &(PS2.get_RightY <> 255) & (PS2.get_RightX <> 0) & (PS2.get_RightX <> 255)
      xbee.tx(0)
      pst.str(String("Sent: 0",13))
    else
      if (PS2.get_RightY == 255)
        xbee.tx(1)                                               
        pst.str(String("Sent: 1",13))
      if (PS2.get_RightY == 0)
        xbee.tx(2)
        pst.str(String("Sent: 2",13))
      if (PS2.get_RightX == 255)
        xbee.tx(4)
        pst.str(String("Sent: 4",13))
      if (PS2.get_RightX == 0)
        xbee.tx(5)
        pst.str(String("Sent: 5",13))

    {else
      if (PS2.get_RightY == 255)
        xbee.tx(6)   
    if (PS2.get_Data1 == $F7FF5A73)
      xbee.tx(3)
      pst.str(String("Sent: 3",13))
    else
      if (PS2.get_Data1 == $FDFF5A73)
        xbee.tx(4)
        pst.str(String("Sent: 4",13))
    if (PS2.get_Data1 == $FFF75A73)
      pst.str(String("Pressed: START",13))
      xbee.start(4,3,0,9600)
    if (PS2.get_Data1 == $FFFE5A73)
      pst.str(String("Pressed: SELECT",13))
      xbee.stop
    if (PS2.get_Data1 == $FFEF5A73)
      xbee.tx(1)
      pst.str(String("Sent: 1", 13))
    else
      if (PS2.get_Data1 == $FFBF5A73)
        xbee.tx(2)
        pst.str(String("Sent: 2",13)) }
     
    
PUB MoveMotor(Pin, position)                              
  dira[Pin]~~                                   
  repeat                                        
    outa[Pin]~~                                 
    waitcnt((clkfreq/100_000)*position+cnt)    
    outa[Pin]~                                 
    waitcnt(clkfreq/100+cnt)